/***************************************************************************
 创建者: 林逢达
 开始时间: 2016.11.28
 copyright: (C) 同川科技
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)

 ***************************************************************************
 *  @file coordinate.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef COORDINATE_H
#define COORDINATE_H

#include "domparser.h"
#include "GeneralDefine.h"
#include "pointdata.h"
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/frames.hpp>
#include "sixjointrobot.h"
#include "messagelog.h"
#include "datarecord.h"

using namespace KDL;

class Coordinate:  public DomParser
{
private:
    /**
     * @brief robotType, 机器人类型
     */
    int robotType;
    int internalNum;//内部轴数量
    int robotId;
    QVector<JointAttribute> *robotJoints;
    Chain robotChain;   
    JntArray jointsMax;//关节正限位
    JntArray jointsMin;//关节负限位
    ChainFkSolverPos_recursive *fkSolverPos;
    ChainFkSolverVel_recursive *fkSolverVel;
    ChainIkSolverVel_pinv *ikSolverVel_pinv;
    ChainIkSolverPos_LMA *ikSolverPos_LMA;
    ChainIkSolverPos_NR *ikSolverPos_NR;
    ChainIkSolverPos_NR_JL *ikSolverPos_NR_JL;
    DataRecord* dataRecord;

    void addMsg(int messageLevel,
                std::string componentName,
                std::string messageType,
                int messageCode,
                int _robotId,
                int parameter1 = 0,
                int parameter2 = 0,
                int parameter3 = 0,
                int parameter4 = 0);

public:
    Coordinate(int _robotType, int _internalNum, int _robotId, QVector<JointAttribute> *_robotAttribute);
    ~Coordinate();
    void initRobotChain();
    Rotation TaitBryanXYZ(double psi, double sita, double fai);
    void getTaitBryanXYZ(const Rotation& rotMat, double& psi, double& sita, double& fai);
    int setToolCoordinateBy6Points(ToolCoordinateInfo& initInfo);
    int setToolCoordinateBy3Points(ToolCoordinateInfo& initInfo);
    int setToolCoordinateByXyzwpr(ToolCoordinateInfo& initInfo);
    int setUserCoordinateBy3Points(UserCoordinateInfo& initInfo, const Frame& tcpForward);
    int setUserCoordinateBy4Points(UserCoordinateInfo& initInfo, const Frame& tcpForward);
    int setUserCoordinateByXyzwpr(UserCoordinateInfo& initInfo);
    int getRotationBy3Points(const Matrix3f& rf3, Rotation& rotMat);
    MatrixXf getSchmidtMatrix(const MatrixXf& Sd);
    int getPointBy3Mat(const Frame& T1, const Frame& T2, const Frame& T3, Vector& point);
    Frame pointProToFrame(const PointPro& pointPro);
    Frame jntArrayToFrame(const JntArray& jntArray);
    PointPro frameToPointPro_CART(const Frame& f, int ufValue=0, int utValue=0);
    int frameToPointPro_JOINT(const Frame& frame, PointPro &pointOut, int ufValue=0, int utValue=0);
    int getMultiConfig(const std::vector<float> positionValue, std::string &configString, std::vector<int> &configInt);
    int getFkPos(const JntArray& q_in, Frame& pos);
    int getFkVel(const JntArrayVel& qvel_in, FrameVel& vel);
    int getIkPos_NR(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkPos_LMA(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkPos_NR_JL(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    /**
     * @brief getIkVel_pinv
     * @param q_in, the current position of joint
     * @param v_in
     * @param jointsDot
     * @return
     */
    int getIkVel_pinv(const JntArray& q_in, const Twist& v_in, JntArray& jointsDot);

    int getIkPos_quick(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkPos_SCARA(const JntArray& q_init, const Frame& p_in, JntArray& joints);
   // int getIkPos_PALLET(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkPos_SIX(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkPos_SIX_FUT(int fut[6], const Frame& p_in, JntArray& joints);
    //int getIkPos_ADAPT(const JntArray& q_init, const Frame& p_in, JntArray& joints);

     SixJointRobot* sixJointRobot;
     SixJointRobot* sixJointRobotForThread2;

private:
     int transConfigString2ConfigInt(QString configString, int fut[6]);
};

#endif // COORDINATE_H
